
/* 接收命令数据  modbus */

#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>

#ifndef _MSC_VER
#include <unistd.h>
#endif
#if defined(_WIN32)
#define close closesocket
#endif

#include "modbus/modbus.h"

#include "rclcpp/rclcpp.hpp"
#include "basic_sprayer_interfaces/msg/human_cmd.hpp" // 控制信号

#include <signal.h>

using namespace std::chrono_literals;

class humanNode : public rclcpp::Node
{
public:
    humanNode()
        : Node("humanNode")
    {
        publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::HumanCmd>("humancmd", 10);
    }
    void accept_mdobus(void);

private:
    rclcpp::Publisher<basic_sprayer_interfaces::msg::HumanCmd>::SharedPtr publisher_;
};

bool stop = false;     //停止信号
void sig_handler(int signum)
{
    if (signum == SIGINT || signum == SIGTERM) {
        stop = true;
    }
}

void humanNode::accept_mdobus(void)
{
    modbus_t *ctx = NULL;
    /*接收任何ip的数据*/
    ctx = modbus_new_tcp(NULL, 9999); //设置端口为9999
    int s = modbus_tcp_listen(ctx, 1);
    modbus_tcp_accept(ctx, &s);
    int rc;
    modbus_mapping_t *mb_mapping = NULL;
    mb_mapping = modbus_mapping_new(0, 0, MODBUS_MAX_READ_REGISTERS, 0);
    if (mb_mapping == NULL)
    {
        fprintf(stderr, "Failed to allocate the mapping: %s\n",
                modbus_strerror(errno));
        modbus_free(ctx);
        return;
    }
    mb_mapping->tab_registers[0] = 0;
    for (;;)
    {
        uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
        rc = modbus_receive(ctx, query);
        if (rc > 0)
        {
            modbus_reply(ctx, query, rc, mb_mapping);
            /*处理接收到的数据*/
            auto message = basic_sprayer_interfaces::msg::HumanCmd();
            message.header.stamp = this->now();    // 给定时间
            if (mb_mapping->tab_registers[0] != 0)
            {
                message.cmd_switch = message.CMD_START;
                int16_t speed = mb_mapping->tab_registers[1];
                message.velocity = speed / 100.f; // m/s
                int16_t angle = mb_mapping->tab_registers[2];
                message.angle = angle / 100.f; //单位°  deg
            }
            else
            {
                message.cmd_switch = message.CMD_STOP;
                int16_t speed = mb_mapping->tab_registers[1];
                message.velocity = speed / 100.f; // m/s
                int16_t angle = mb_mapping->tab_registers[2];
                message.angle = angle / 100.f; //单位°  deg
            }
            publisher_->publish(message);
        }
        else if (rc == -1)
        {
            /* Connection closed by the client or error */
            break;
        }

        if(stop == true){
            break;
        }
    }
    printf("Quit the loop: %s\n", modbus_strerror(errno));

    if (s != -1)
    {
        close(s);
    }
    modbus_mapping_free(mb_mapping);
    modbus_close(ctx);
    modbus_free(ctx);
}

/**
* 
* @author 何思伟
* @param 
*
* @note 
*
* @return 
*/


int main(int argc, char *argv[])
{
    struct sigaction sa;
    memset(&sa, 0, sizeof(sa));
    sigset_t mask;
    memset(&mask,0,sizeof(sigset_t));    //要手动清空

    sa.sa_handler = sig_handler;
    sigemptyset(&sa.sa_mask);
    sigemptyset(&mask);

    if (sigaction(SIGTERM, &sa, NULL) < 0) {
        printf("sigaction: %s\n", strerror(errno));
        return -1;
    }

    if (sigaction(SIGINT, &sa, NULL) < 0) {
        printf("sigaction: %s\n", strerror(errno));
        return -1;
    }

    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    /* make sure these signals are unblocked */
    if (sigprocmask(SIG_UNBLOCK, &mask, NULL)) {
        printf("sigprocmask: %s", strerror(errno));
        return -1;
    }


    /*创建 ros */
    rclcpp::init(argc, argv);
    auto pub_node = std::make_shared<humanNode>();
    while (stop == false)
    {
        pub_node->accept_mdobus();
        printf("close once，restart!\n");
    }
    /*创建线程*/
    rclcpp::shutdown();
}
